/********************************************************************************
* This program is free software; you can redistribute it and/or modify it under *
* the terms of the GNU General Public License as published by the Free Software *
* Foundation; either version 2 of the License, or (at your option) any later 	*
* version. This program is distributed in the hope that it will be useful, but 	*
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more *
* details. You should have received a copy of the GNU General Public License 	*
* along with this program; if not, write to the Free Software Foundation, Inc., *
* 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.						*
* (c) Copyright 2012 José Delgado Pérez, josedelgado@ula.ve josed43@gmail.com.	*				*
********************************************************************************/

/**
 @file main.h
 @brief Implemetación, recibe ordenes desde el PC y usa la libreria para ejecutar estas ordenes y enviá la respuesta al PC.
 @date Diciembre, 2012.
 @author José Delgado Pérez, josedelgado@ula.ve josed43@gmail.com.
 @version 0.9
*/

#include <Arduino.h>
#include "definicion.h"
#include "biblioteca/percepcion.h"
#include "Servo.h"

percepcionLR2 sistemaPercepcion;
char sbuf[4];
int id, anbgulo = 90;
unsigned short int distanciaUS = 0, *distanciasUS;
uint8_t distanciaIR, *distanciasIR;
unsigned char wbuf[12];
int  angulo =20;
char prueba = '1';
Servo servoPrueba;
int entrada = 0;


void serializeShort(uint8_t *buf, unsigned short int value) {
   union{
	   uint8_t c[2];
       unsigned short int val;
   } unsigned_short_int;
   if (buf == NULL) { return; }
   unsigned_short_int.val = value;
   buf[0] = unsigned_short_int.c[1];
   buf[1] = unsigned_short_int.c[0];
}

void setup() {
	servoPrueba.attach(3);
	sistemaPercepcion.inicializarSistemaPerecepcion(&servoPrueba);
	Serial.begin(9600);
}

void loop() {
 if( Serial.available() > 0 ){
		Serial.readBytesUntil(BYTE_FIN_COMADO, sbuf, SIZE_MAX_COMADO);
		if(sbuf[0] == BYTE_INICIO_COMADO){
			switch (sbuf[1]) {
				case OBTENER_MEDIDA_US:
					id = (int)sbuf[2];
					distanciaUS = sistemaPercepcion.obtenerMedidaUS(id);
					serializeShort(wbuf, distanciaUS);
					Serial.write(wbuf,2);
					break;
				case OBTENER_MEDIDA_IR:
					id = (int)sbuf[2];
					distanciaIR = sistemaPercepcion.obtenerMedidaIR(id);
					wbuf[0] = (uint8_t)distanciaIR;
					Serial.write(wbuf,1);
					break;
				case OBTENER_MEDIDA_UST:
					anbgulo = (int)sbuf[2];
					distanciaUS = sistemaPercepcion.obtenerMedidaUSTrasero(anbgulo);
					serializeShort(wbuf,distanciaUS);
					Serial.write(wbuf,2);
					break;
				case OBTENER_BARRIDO_FRONTAL_US:
					distanciasUS = sistemaPercepcion.obtenerBarridoFrontalUS();
					for (int i = 0; i < 6; ++i) {
						serializeShort(&wbuf[2*i],distanciasUS[i]);
					}
					Serial.write(wbuf,12);
					break;
				case OBTENER_BARRIDO_FRONTAL_IR:
					distanciasIR = sistemaPercepcion.obtenerBarridoFrontalIR();
					for (int i = 0; i < 6; ++i) {
						wbuf[i] = (uint8_t)distanciasIR[i];
					}
					Serial.write(wbuf,6);
					break;
				case OBTENER_BARRIDO_TRASERO:
					distanciasUS = sistemaPercepcion.obtenerBarridoTraseroUS();
					for (int i = 0; i < 6; ++i) {
						serializeShort(&wbuf[2*i],distanciasUS[i]);
					}
					Serial.write(wbuf,10);
					break;
				default:
				//	comSerial.limpiarBufer();
					break;
			}
		}else{
			Serial.flush();
		}
	}
}

int main(void){
	init();
	setup();
	for (;;)
		loop();
	return 0;
}

